package edu.asu.sapa.ground;


import java.util.Arrays;

import edu.asu.sapa.Planner;
import edu.asu.sapa.ground.update.Constraint;
import edu.asu.sapa.ground.update.ConstraintWrapper;
import edu.asu.sapa.ground.update.Context;
import edu.asu.sapa.ground.update.Load;
import edu.asu.sapa.ground.update.Update;
import edu.asu.sapa.ground.update.UpdateWrapper;
import edu.asu.sapa.utils.Utility;

/**
 * State.java: Store the information about the current state such as : (1) Set
 * of predicates that is true & resource information (stored in a GState
 * structure) (2) Set of future events and the occuring time points. (3) Set of
 * actions leading to this state and their execution time. (4) Set of
 * predicates/functions that need to be protected and their end time points. (5)
 * Time at which we measure this state (6) Distance to the goal (heuristically
 * measured) (7) Other information such as total execution costs, or total
 * duration etc.
 */
public class State implements Cloneable, Comparable<State> {

	// managed by constructor
	public float time;
	public PropDB propDB;
	public FluentDB fluentDB;
	public UpdateQ<Update> events;
	public ConstraintWrapper[] constraints;
	public int constraintsSize;

	public int g;
	public float gCost;
	//public float gReward;

	public State parent;
	public int operator;
	public Context c;

	// managed by Planner.evaluate()
	// public float distance;
	// public float a;
	public int h;
	public float hCost;
	public float hReward;
	public Operator[] potentialOperators;
	public int[] rp;

	public State(float time) {
		operator = -1;
		c = null;
		parent = null;
		
		fluentDB = new FluentDB();
		propDB = new PropDB();

		events = new UpdateQ();
		constraints = new ConstraintWrapper[0];
		constraintsSize=0;

		this.time = time;
		
		gCost = 0f;
		g=0;
	}

	public State(State sp) {
		g = sp.g;
		h = sp.h;
		hCost = sp.hCost;
		gCost = sp.gCost;
		hReward = sp.hReward;

		operator = sp.operator;
		c = sp.c;
		parent = sp.parent;

		if (sp.propDB != null)
			propDB = new PropDB(sp.propDB);
		else
			propDB = null;
		if (sp.fluentDB != null)
			fluentDB = new FluentDB(sp.fluentDB);
		else
			fluentDB = null;
		
		time = sp.time;

		events = new UpdateQ(sp.events);
		constraintsSize = sp.constraintsSize;
		constraints = new ConstraintWrapper[constraintsSize];
		System.arraycopy(sp.constraints,0,constraints,0,constraintsSize);
		
	}

	public State(Context c) {
		this.c = c;
		this.time = c.start;
		this.operator = c.id;
		parent = null;
		
		fluentDB = new FluentDB();
		propDB = new PropDB();

		events = new UpdateQ();
		constraints = new ConstraintWrapper[0];
		constraintsSize=0;

		gCost = 0f;
		g=0;
	}

	public float benefit() {
		return 0 /*hReward - hCost*/ - gCost;
	}
	public float estimate() {
		return hReward - hCost - gCost;
	}

	@Override
	public Object clone() {
		State s = null;
		try {
			s = (State) super.clone();
		} catch (CloneNotSupportedException e) {
			// assert false;
		}

		s.propDB = (PropDB) propDB.clone();
		s.fluentDB = (FluentDB) fluentDB.clone();

		s.events = (UpdateQ) events.clone();
		
		s.constraints = new ConstraintWrapper[constraintsSize];
		System.arraycopy(constraints,0,s.constraints,0,constraintsSize);
		
		return s;
	}

	public int compareTo(State s) {
		// for speed, leave these as assertions instead of ifs
		// assert this != null;
		// assert s != null;
		// if (distance < s.distance)
		// return -1;
		// if (distance > s.distance)
		// return 1;
		return 0;
	}

	public void sort() {
		
	}
	
	// A quicksort that bottoms out to an insertion sort below a given threshold on size
	// sorts on id, replicating swaps in the other arrays
	// could change declaration of flags and times to Object[] to make it _slightly_ more general...
    public static final void sort(int[] id, int[] value, int[] futureSize, byte[][] flags, float[][] times, int left, int right) {
    	int u,v;
		int i,j,k;
		int val, fs;
		byte[] f;
		float[] t;
        if (right - left <  7) {
        	for(i=left+1;i<=right;++i) {
        		j=i-1;
        		k=i;
        		u = id[j];
        		v = id[i];
        		val = value[i];
        		fs = futureSize[i];
        		f = flags[i];
        		t = times[i];
        		while (j >= left && u > v) {
        			id[k] = u;
        			value[k] = value[j];
        			futureSize[k] = futureSize[j];
        			flags[k] = flags[j];
        			times[k] = times[j];
        			--j;
        			--k;
        			u = id[j];
        		}
        		id[k] = v;
        		value[k] = val;
        		futureSize[k] = fs;
        		flags[k] = f;
        		times[k] = t;
        	}
        } else {
        	
            i = left;
            j = right;
            u = id[i];
            v = id[j];
            if (u > v) {
            	// k = id[i];
        		val = value[i];
        		fs = futureSize[i];
        		f = flags[i];
        		t = times[i];
        		
        		id[i] = v; // id[i] = id[j]
        		value[i] = value[j];
        		futureSize[i] = futureSize[j];
        		flags[i] = flags[j];
        		times[i] = times[j];
        		
        		id[j] = u; // id[j] = k;
        		value[j] = val;
        		futureSize[j] = fs;
        		flags[j] = f;
        		times[j] = t;
        		
        		v=u;
            }
            do {				// find item on left to swap
                u=id[++i];		// id[right] acts as sentinel
            } while (u < v);
            do {
                u=id[--j];		// find item on right to swap
            } while (u > v); 	// id[left] acts as sentinel
            
            while (i<j) {
            	k = id[i];
        		val = value[i];
        		fs = futureSize[i];
        		f = flags[i];
        		t = times[i];
        		
        		id[i] = u; // id[i] = id[j]
        		value[i] = value[j];
        		futureSize[i] = futureSize[j];
        		flags[i] = flags[j];
        		times[i] = times[j];
        		
        		id[j] = k;
        		value[j] = val;
        		futureSize[j] = fs;
        		flags[j] = f;
        		times[j] = t;
        		
                do {				// find item on left to swap
                    u=id[++i];		// id[right] acts as sentinel
                } while (u < v);
                do {
                    u=id[--j];		// find item on right to swap
                } while (u > v); 	// id[left] acts as sentinel
            }
            
        	k = id[i];
    		val = value[i];
    		fs = futureSize[i];
    		f = flags[i];
    		t = times[i];
    		
    		id[i] = v; // id[i] = id[right]
    		value[i] = value[right];
    		futureSize[i] = futureSize[right];
    		flags[i] = flags[right];
    		times[i] = times[right];
    		
    		id[right] = k;
    		value[right] = val;
    		futureSize[right] = fs;
    		flags[right] = f;
    		times[right] = t;
    		
            sort(id, value, futureSize, flags, times, left, i-1);
            sort(id, value, futureSize, flags, times, i+1, right);
        }
    }


    // only try this on quiescent states...
	public boolean equals(State s) {

		if (operator > 0 && s.operator == 0)
			return false;
		if (operator == 0 && s.operator > 0)
			return false;
		
		if (propDB != null) {
			if (s.propDB == null || !propDB.equals(s.propDB))
				return false;
		} else {
			if (s.propDB != null)
				return false;
		}
		
		if (fluentDB != null) {
			if (s.fluentDB == null || !fluentDB.equals(s.fluentDB))
				return false;
		} else {
			if (s.fluentDB != null)
				return false;
		}
		return true;
	}

	@Override
	public boolean equals(Object obj) {
		if (obj instanceof State)
			return equals((State)obj);
		return false;
	}

	@Override
	public int hashCode() {
		return (operator > 0 ? 1 : 0 ) ^ (propDB == null ? 0 : propDB.hashCode()) ^ (fluentDB == null ? 0 : fluentDB.hashCode());
	}

	public float getCost() {
		return gCost;
	}

	public FluentDB getFluentDB() {
		return fluentDB;
	}

	public boolean quiescent() {
		if (events.size <= 0 || events.table[0].time >= Float.POSITIVE_INFINITY)
			return true;
		return false;
	}
	
	public float getNextTime() {
		float nextTime;

		if (operator > 0)
			nextTime = time+Planner.EPSILON;
		else if (events.size > 0)
			nextTime = events.table[0].time;
		else
			nextTime = Float.POSITIVE_INFINITY;
		
		return nextTime;
	}

	public Operator[] getPotentialOperators() {
		return potentialOperators;
	}

	public PropDB getPropDB() {
		return propDB;
	}

	public float getTime() {
		return time;
	}
	
	public final boolean constraintsHoldAt() {
		int sz = constraintsSize;
		ConstraintWrapper[] constraints = this.constraints;
		for(int i=0; i<sz; ++i) {
			if (!constraints[i].holdsAt(this))
				return false;
		}
		return true;
	}
	
	public final boolean constraintsHoldBefore() {
		int sz = constraintsSize;
		ConstraintWrapper[] constraints = this.constraints;
		for(int i=0; i<sz; ++i) {
			if (!constraints[i].holdsBefore(this))
				return false;
		}
		return true;
	}
	
	/**
	 * Move forward to the first event's time, and update the current-state
	 * according to earliest events
	 */
	public boolean advance() {
		boolean possible = true;
		float next;

		if (operator > 0) {
			next = time+Planner.EPSILON;
			possible &= constraintsHoldAt(); //can avoid checking if we guarantee using the PG to check, but RP-execution skips that
			time = next;
			possible &= events.processBefore(this,next);
			possible &= constraintsHoldBefore();
		}
		else if (events.size > 0) {
			next = events.table[0].time;
			possible &= constraintsHoldAt(); //can avoid checking if we guarantee using the PG to check, but RP-execution skips that
			time = next;
			possible &= events.processBefore(this,next);
			possible &= constraintsHoldBefore();
		} else {
			next = Float.POSITIVE_INFINITY;
			//possible &= constraintsHoldAt(); //State won't change as there are no events
			time = next;
			//possible &= events.processBefore(this,next);
			possible &= constraintsHoldBefore();
		}

		
		// for my interpretation of PDDL, checking over all constraints can happen after updating by each action
		// so that Planner.evaluate() can already ensure State.constraintsHoldAt()
		// i.e., over all p == at start + over interim + at end p
		// and at start == before start (also at end == before end)
		// so over all conditions have to hold before executing an action.
		//   An exception, for the case of certain benchmarks, is actions achieving their own over-all conditions.  These (the conditions) are removed during
		//   grounding [replaced by durative assignments].
		//
		//  The constraint checking currently happens in PG.costPropagation, since it might anyways want to look
		//  at the constraints for the purposes of analysis.
		
		operator = 0;
		c = null;
		
		
		if (!possible)
			gCost += Float.POSITIVE_INFINITY;
		return possible;
	}

	
	public boolean advance(float nextTime) {
		float next = getNextTime();
		
		boolean possible = true;
		while (next <= nextTime) {
			possible &= constraintsHoldAt();
			time = next;
			possible &= events.processBefore(this,next);
			possible &= constraintsHoldBefore();
			
			next = getNextTime();
		}

		time = nextTime;
		operator = 0;
		c = null;
		
		return possible;
	}
	
	public void setFluentDB(FluentDB fluentDB) {
		this.fluentDB = fluentDB;
	}

	public void setPotentialOperators(Operator[] potentialOperators) {
		this.potentialOperators = potentialOperators;
	}

	public void setPropDB(PropDB propDB) {
		this.propDB = propDB;
	}

	public void setTime(float time) {
		this.time = time;
	}

	@Override
	public String toString() {
		return ":now " + time + "\n" + (propDB == null ? "" : propDB.toString()) + "\n" + (fluentDB == null ? "" : fluentDB.toString() + "\n");
	}

	@Deprecated
	public boolean update(Operator a) {
		
//		if (a.id <= maxID)
//			return false;
		
		// if (!resApplicable(a) return false;

		operator = a.id;
		c = a.makeContext(this);
		
		// if (a.duration.type == 1)
		// return updateConstantDuration(a, dur);

		// don't really need to check at start conditions and tests again
		// planning graph monitors conditions and search code calls
		// applicable before copying stuff.
		// but for safety, and because it costs very little, track possibility as below
		// (instead of with early exit if's, because cmp-jmp is much more expensive than
		// and + assign
		boolean possible = true;
		
		for (Load l : a.contents) {
			possible &= l.load(this,c);
		}
		
		if (!possible)
			return false;

		g += 1;
		gCost += c.cost;

		// should do something about protecting duration and cost fluents
		
//		time += Planner.EPSILON;
		return true;
//		return moveForward(time + Planner.EPSILON);
	}

	@Deprecated
	public boolean applicable(Operator a) {
		final Context c = new Context();
		if (a.id <= operator)
			return false;
		
		float start = time;
		float dur = a.getDuration(this,null);
		float finish = start + dur;
		c.id = a.id;
		c.start = time;
		c.duration = dur;
		c.finish = finish;
		c.cost = a.getCost(this,c);

		boolean possible = true;
		for (Load l : a.contents) {
			possible &= l.applicableBefore(this,c);
		}
		
		if (!possible)
			return false;

		return true;
	}

	public boolean addConstraint(Constraint constraint, Context c) {
		ConstraintWrapper w = new ConstraintWrapper(c,constraint);
		if (constraintsSize >= constraints.length) {
			ConstraintWrapper[] oldA = constraints;
			int s = constraintsSize;
			constraints = new ConstraintWrapper[s+2];
			System.arraycopy(oldA,0,constraints,0,s);
		}
		constraints[constraintsSize++] = w;
		return true;
	}
	public boolean removeConstraint(Constraint constraint, Context c) {
		int s = constraintsSize;
		ConstraintWrapper[] a = constraints;
		ConstraintWrapper w;
		for (int i=s-1; i>=0; --i) {
			w = a[i];
			if (w.c == c && w.constraint == constraint) {
				Utility.remove(a,i);
				return true;
			}
		}
		return false;
	}

	public boolean analyzeStatic() {
		boolean possible = true;
		int i, size;
		UpdateWrapper<Update>[] table;
		table = events.table;
		size = events.size;
		for (i=0;i<size;++i) {
			possible &= table[i].event.analyzeStatic(Float.POSITIVE_INFINITY);
		}
		
		return true;
	}

	public State child(Operator a, Context c) {
		State ret = new State(this);
		ret.parent = this;
		if (a.update(ret,c))
			return ret;
		return null;
	}
	
}
